#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTMotor)
#pragma config(Hubs,  S2, HTServo,  HTServo,  HTServo,  HTServo)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Motor,  motorA,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorC,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  mtr_S1_C1_1,     motorFL,       tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorFR,       tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     motorBL,       tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     motorBR,       tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     motorFan1,     tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     motorFan2,     tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C4_1,     motorSweeper,  tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C4_2,     motorEnc,      tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S2_C1_1,    servoFL,              tServoContinuousRotation)
#pragma config(Servo,  srvo_S2_C1_2,    servoFR,              tServoContinuousRotation)
#pragma config(Servo,  srvo_S2_C1_3,    servo1,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_4,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_5,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_6,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S2_C2_1,    servoBL,              tServoContinuousRotation)
#pragma config(Servo,  srvo_S2_C2_2,    servoBR,              tServoContinuousRotation)
#pragma config(Servo,  srvo_S2_C2_3,    servoLoader,          tServoStandard)
#pragma config(Servo,  srvo_S2_C2_4,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S2_C2_5,    servo6,               tServoNone)
#pragma config(Servo,  srvo_S2_C2_6,    servo7,               tServoNone)
#pragma config(Servo,  srvo_S2_C3_1,    servoSweeperArm,      tServoStandard)
#pragma config(Servo,  srvo_S2_C3_2,    servoSweeper1,        tServoContinuousRotation)
#pragma config(Servo,  srvo_S2_C3_3,    servoSweeper2,        tServoContinuousRotation)
#pragma config(Servo,  srvo_S2_C3_4,    servo8,               tServoNone)
#pragma config(Servo,  srvo_S2_C3_5,    servo9,               tServoNone)
#pragma config(Servo,  srvo_S2_C3_6,    servo10,              tServoNone)
#pragma config(Servo,  srvo_S2_C4_1,    servoTubeWinch,       tServoStandard)
#pragma config(Servo,  srvo_S2_C4_2,    servoTubeSlide,       tServoContinuousRotation)
#pragma config(Servo,  srvo_S2_C4_3,    servo13,              tServoNone)
#pragma config(Servo,  srvo_S2_C4_4,    servo14,              tServoNone)
#pragma config(Servo,  srvo_S2_C4_5,    servo15,              tServoNone)
#pragma config(Servo,  srvo_S2_C4_6,    servo16,              tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "../JoystickDriver.c"
#include "../../Library/drive_modes/simple_swerve_4m.c"

//return the position of a drive assembly CR servo in degrees
float GetCRServoPosition(int corn)
{
	if (corn == 0)
	{
		return ((float)nMotorEncoder[motorFL] * 0.125);
	}
	else if (corn == 1)
	{
		return ((float)nMotorEncoder[motorFR] * 0.125);
	}
	else if (corn == 2)
	{
		return ((float)nMotorEncoder[motorBL] * 0.125);
	}
	else if (corn == 3)
	{
		return ((float)nMotorEncoder[motorBR] * 0.125);
	}
	else
	{
		return 0;
	}
}

task main()
{
	RegisterDriveHardware(
	motorFL, motorFR,
	motorBL, motorBR,
	servoFL, servoFR,
	servoBL, servoBR,
	);

	float Kp = 0.009;
	float Ki = 0.008;
	float Kd = 0.005;
	float errorPrevSum[4] = {0.0, 0.0, 0.0, 0.0};
	float errorPrev[4] = {0.0, 0.0, 0.0, 0.0};
	float error[4];
	float ang[4] = {0.0, 0.0, 0.0, 0.0};
	float newAng[4] = {0.0, 0.0, 0.0, 0.0};
	float n[4] = {0.0, 0.0, 0.0, 0.0};
	float angPrev[4] = {0.0, 0.0, 0.0, 0.0};
	float newAngPrev[4] = {0.0, 0.0, 0.0, 0.0};
	float servoSpeed[4] = {0.0, 0.0, 0.0, 0.0};
	int reverseMotorFactor[4] = {1,1,1,1,};

	for (int p = 0; p < 4; p++)
	{
		Drive[p].servoSpeed = 0;
	}
	waitForStart();
	float joyDistance;
	float joyAngle;
	float joyX;
	float joyY;
	float joyZ;
	float MOTOR_SPEED = 90;
	float power;
	while(true)
	{
		joyX = joystick.joy1_x2;
		joyY = joystick.joy1_y2;
		joyZ = joystick.joy1_x1;
		joyDistance = sqrt( pow(joyX, 2) + pow( joyY, 2) );
		joyAngle = 57.3 * atan2(joyY, joyX);

		for (int p = 0; p < 4; p++)
		{

			if (joyDistance > 20)
			{
				ang[p] = joyAngle;
			}
			else if (joyZ > 20)
			{
				ang[0] = 45;
				ang[1] = 45;
				ang[2] = -45;
				ang[3] = 45;
			}

			error[p] = ang[p] + GetCRServoPosition(p) + n[p];

			while(error[p] > 90)
			{
				n[p] += 180 * sgn(error[p]);
				if (abs(n[p]) > 1440)
				{
					n[p] -= 180 * sgn(error[p]);
				}
				Drive[p].reverseMotorPower *= -1;
				error[p] += n;
			}

			/*
			if (abs(error[p]) > 180)
			{
				n[p] -= sgn(error[p]) * 360;
				error[p] = ang[p] + GetCRServoPosition(p) + n[p];
			}
			*/

			servoSpeed[p] = ( Kp * error[p] ) + ( Ki * errorPrevSum[p] ) + ( Kd * (error[p] - errorPrev[p]) );
			servo[Drive[p].pServo] = 128 + (-128 * servoSpeed[p]);
			errorPrev[p] = error[p];
			errorPrevSum[p] = errorPrevSum[p] + errorPrev[p] * 0.005;
		}
		if (abs(joyZ) > 20)
		{
			power = 0.7 * joyZ - 20;
		}
		else if (joyDistance > 20)
		{
			power = joyDistance - 20;
		}
		else
		{
			power = 0;
		}

		motor[motorFL] = power * reverseMotorFactor[0];
		motor[motorFR] = power * reverseMotorFactor[1] * -1;
		motor[motorBL] = power * reverseMotorFactor[2];
		motor[motorBR] = power * reverseMotorFactor[3] * -1;
		wait1Msec(100);
	}
}
